#include <opencv2/opencv.hpp>
#include <iostream>
#include <fstream>

using namespace cv;
using namespace std;

int main(int argc, char **argv) {
    ifstream fin("calibdata.txt");   /*标定用图像文件路径*/
    ofstream fout("calibresult.txt"); /*保存标定结果的文件*/

    //读取每一幅图像，从中提取角点，然后对角点进行亚像素精确化

    cout << "开始提取角点...";
    int image_count = 0; /*图像数量*/
    Size image_size; /*图像尺寸*/
    Size board_size = Size(6, 9);  //标定板每行列角点数

    vector<Point2f> image_points_buf; /*缓存每幅图像上检测到的角点*/
    vector<vector<Point2f>> image_point_seq;  /*保存所有检测到的角点*/

    string filename;
    while (getline(fin, filename)) {
        image_count++;
        // 用于观察检验输出
        cout << "image_count = " << image_count << endl;
        Mat grayImg = imread(filename, IMREAD_GRAYSCALE);  //读入灰度图

        if (image_count == 1) {
            // 读入第一张图片时获取图像的宽高信息
            image_size.width = grayImg.cols;
            image_size.height = grayImg.rows;

            cout << "image_size.width = " << image_size.width << endl;
            cout << "image_size.height = " << image_size.height << endl;
        }
        if (0 == findChessboardCorners(grayImg, board_size, image_points_buf)) /*输入图像必须是8位灰度或彩色图像*/
        {
            cout << "can not find chessboard corners!\n";  //没有找到全部角点
            exit(1);
        } else {
            Mat rgbImg;
            cvtColor(grayImg, rgbImg, CV_GRAY2BGR);
            /*亚像素精确化*/
            find4QuadCornerSubpix(grayImg, image_points_buf, Size(11, 11));  //粗提取角点 精确化
            /*保存亚像素点*/
            image_point_seq.push_back(image_points_buf);
            /*绘制出检测到的角点(彩色)*/
            drawChessboardCorners(rgbImg, board_size, image_points_buf, true);

            imshow("Camera Calibration", rgbImg);
            waitKey(500);  //暂停0.5s
        }
    }
    int total = image_point_seq.size();  //读取的总的图像数
    cout << "total = " << total << endl;
    int CornerNum = board_size.width * board_size.height;  /*总的角点数*/

    for (int i = 0; i < total; i++) {
        if (0 == i % CornerNum) {
            int j = -1;
            j = i / CornerNum;
            int k = j + 1;
            cout << "--> 第 " << k << "图片的数据 -->" << endl;
        }
        if (0 == i % 3) {
            cout << endl;
        } else {
            cout.width(10);
        }
        //输出所有图像中第一个角点坐标
        cout << "-->" << image_point_seq[i][0].x;
        cout << "-->" << image_point_seq[i][0].y;
    }
    cout << "角点提取完成！\n";

    //摄像机标定
    cout << "开始标定..." << endl;
    Size square_size = Size(50, 50); /*棋盘格大小50mm*/

    vector<vector<Point3f>> object_points; /*保存棋盘上的坐标点*/
    vector<int> point_counts; //每幅图像中的角点的数量

    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /*摄像机内参数矩阵*/
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));  /*摄像机的5个畸变系数：k1,k2,p1,p2,k3*/

    vector<Mat> tvecsMat; /*存储每幅图像的旋转向量*/
    vector<Mat> rvecsMat; /*存储每幅图像的平移向量*/


    /*设置棋盘点的三维坐标*/
    for (int t = 0; t < image_count; t++) {
        vector<Point3f> tempPointSet;
        for (int i = 0; i < board_size.height; i++)   //一定从行开始（刚开始没注意写错了，找了半天）
        {
            for (int j = 0; j < board_size.width; j++) {
                Point3f realPoint;
                //注意x,y对应关系
                realPoint.x = j * square_size.width;  //注意棋盘格默认坐标系的位置
                realPoint.y = i * square_size.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);  /*添加一幅图像的所有角点坐标*/
            }
        }
        object_points.push_back(tempPointSet);
    }
    /*初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板*/
    for (int i = 0; i < image_count; i++) {
        point_counts.push_back(board_size.width * board_size.height);
    }
    /*calibrate camera*/
    calibrateCamera(object_points, image_point_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

    cout << "标定完成！\n";

    //对标定结果进行评价  可以写成一个单独的函数
    cout << "开始评价标定结果.........\n";
    double total_err = 0.0;  //所有图像的平均误差的总和
    double err = 0.0;    /*每幅图像的平均误差*/
    vector<Point2f> image_points2; /*保存重新计算得到的投影点*/

    cout << "\t每幅图像的标定误差：\n";
    fout << "每幅图像的标定误差：\n";

    for (int i = 0; i < image_count; i++) {
        vector<Point3f> tempPointSet = object_points[i];
        /*通过得到摄像机内外参数、对空间的三维点进行重新投影计算，得到新的投影点*/
        projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
        /*计算新的投影点和旧的投影点之间的误差*/
        vector<Point2f> tempImagePoint = image_point_seq[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2); //存储未矫正角点的坐标值
        Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);  //存储重新投影计算得到的投影角点坐标值

        for (int j = 0; j < tempImagePoint.size(); j++) {
            image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
            tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2); /*矩阵范数运算，用来表征矩阵变化的大小*/
        total_err += (err /= point_counts[i]);
        cout << "第" << i + 1 << "幅图像平均误差： " << err << " 像素" << endl;
        fout << "第" << i + 1 << "幅图像平均误差： " << err << " 像素" << endl;  /*输出到对应文件*/

    }
    cout << "总体平均误差: " << total_err / image_count << "像素" << endl;
    fout << "总体平均误差: " << total_err / image_count << "像素" << endl;
    cout << "评价完成" << endl;

    /*保存标定结果*/
    cout << "开始保存标定结果...." << endl;
    Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /*保存每幅图像的旋转矩阵*/

    fout << "相机内参数矩阵: " << endl;
    fout << cameraMatrix << endl << endl;

    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;

    for (int i = 0; i < image_count; i++) {
        fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
        fout << rvecsMat[i] << endl;
        /*将旋转向量对应的转换为旋转矩阵*/
        Rodrigues(rvecsMat[i], rotation_matrix);
        fout << "第" << i + 1 << "幅图像的旋转矩阵： " << endl;
        fout << rotation_matrix << endl;
        fout << "第" << i + 1 << "幅图像的平移矩阵： " << endl;
        fout << tvecsMat[i] << endl << endl;
    }
    cout << "保存完成" << endl;
    fout << endl;

    //显示矫正图像
    Mat src, dst, map1, map2;
    initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), Mat(), image_size, CV_16SC2, map1, map2); /*计算一次数据，多次使用*/
    fin.clear();/*清除读到文件尾标志位*/
    fin.seekg(0, ios::beg);/*定位到文件开头*/

    while (getline(fin, filename)) {
        src = imread(filename, IMREAD_GRAYSCALE);
        remap(src, dst, map1, map2, INTER_LINEAR);
        imshow("矫正图像", dst);
        waitKey(0);
    }

    return 0;
}
